#ifndef TRAJ_SERVER_H_
#define TRAJ_SERVER_H_

#include <ros/ros.h>
#include <mutex>
#include "quad_msgs/Trajectory.h"
#include "quad_msgs/ReplanCheck.h"
#include "quad_msgs/ControlCommand.h"
#include "quad_msgs/Trigger.h"

namespace Planner
{
    class TrajServer
    {
    public:
        TrajServer(const ros::NodeHandle &node_handle);
        ~TrajServer() = default;

        void init();

        // 定时发送轨迹控制点
        void controlCommandCallback(const ros::TimerEvent &event);

        // 定时进行局部规划
        void replanCheckCallback(const ros::TimerEvent &event);

        // 初始化完毕，确认
        bool triggerCallback(quad_msgs::Trigger::Request &request,
                             quad_msgs::Trigger::Response &response);

    private:
        ros::NodeHandle nh_;

        ros::Timer control_command_timer_;
        ros::Timer replan_check_timer_;

        ros::Publisher control_command_pub_;
        ros::Publisher global_traj_pub_;
        ros::Publisher replan_request_traj_pub_;
        ros::Publisher replan_response_traj_pub_;
        ros::ServiceClient replan_client_;
        ros::ServiceServer trigger_server_;

        double hover_time_;         // 在路径起点处悬停时间
        double control_rate_;       // 控制点发送频率
        double replan_rate_;        // 局部规划频率
        double replan_time_window_; // 局部规划的时域滑动窗口大小
        double replan_time_cost_;   // 局部规划预计耗时

        enum MODE
        {
            INIT_FAIL,
            INIT_SUCCESS,
            SAFE_FLY,
            REPLAN_FAIL,
            FINISH_TASK
        };

        struct Status
        {
            int mode;                               // 无人机当前工作状态
            ros::Time init_time;                    // 初始化完成时刻
            int curr_seg;                           // 当前轨迹段数
            double last_time_seq;                   // 上一个分段路径端点的时间戳
            quad_msgs::ControlCommand last_command; // 实现刹车功能，持续发送上一个路径点

            // void operator = (const Status &s)
            // {
            //     mode = s.mode;
            //     init_time = s.init_time;
            //     curr_seg = s.curr_seg;
            //     last_time_seq = s.last_time_seq;
            //     last_command = s.last_command;
            // }
        };

        quad_msgs::Trajectory::Ptr global_traj_msg_;
        Status status_;
        std::mutex mutex_; // 借助mutex和shared_ptr实现copy-on-write
        bool visualization_; // 是否发送可视化信息
        bool replan_triggered_; // 是否开启避障功能
    };
} // namespace Planner

#endif